#include "BodyLink.h"
#include "Marker.h"
#include "Joint.h"

int BodyLink::mBodyLinkCount = BODYLINK_MAGIC_NO;

void BodyLink::init(){
	mChildJoints.clear();
	mHandles.clear();
	mParentJoint=NULL;
	mPrimitive=NULL;
	mID=BodyLink::mBodyLinkCount++;
	mModelID=-1;
	mName = new char[128];
	strcpy(mName, "BodyLink");
	dependsOnDof=NULL;
	mOtherSideLink=NULL;
	mModel=NULL;
}

BodyLink::BodyLink(){
	init();	
}

BodyLink::BodyLink(char *_mName){
	init();
	strcpy(mName, _mName);	
}

BodyLink::~BodyLink(){
	for(int i=0; i<mHandles.size(); i++){
		delete mHandles[i];
	}
	mHandles.clear();
	if(mPrimitive != NULL)
		delete mPrimitive;
	delete mName;
	mChildJoints.clear();
}

Vec4d BodyLink::getWorldCoords(Vec4d *h){
	Vec4d result = *h;
	for(int i=mParentJoint->getNumTransforms()-1; i>=0; i--){
		mParentJoint->getTransform(i)->applyTransformMat(&result);
	}
	if(mParentJoint->getBodyIn()==NULL) return result;
	else return mParentJoint->getBodyIn()->getWorldCoords(&result);
}

Mat4d BodyLink::getLocalTransform(){
	Mat4d result = vl_1;
	for(int i=mParentJoint->getNumTransforms()-1; i>=0; i--){
		mParentJoint->getTransform(i)->applyTransformMat(&result);
	}
	return result;
}

Mat4d BodyLink::getLocalInvTransform(){
	Mat4d result = vl_1;
	for(int i=0; i<mParentJoint->getNumTransforms(); i++){
		mParentJoint->getTransform(i)->applyInvTransformMat(&result);
	}
	return result;
}

Mat4d BodyLink::getWorldTransform(){
	Mat4d result = getLocalTransform();
	
	if(mParentJoint->getBodyIn()==NULL) return result;
	else {
		Mat4d parentTrans= mParentJoint->getBodyIn()->getWorldTransform();
		return parentTrans*result;
	}
}

Mat4d BodyLink::getWorldInvTransform(){
	Mat4d result = getLocalInvTransform();

	if(mParentJoint->getBodyIn()==NULL) return result;
	else {
		Mat4d parentTrans= mParentJoint->getBodyIn()->getWorldInvTransform();
		return result*parentTrans;
	}
}

Vec4d BodyLink::getFirstDerivative(Vec4d *h, Dof *d){
	Vec4d result = *h;
	for(int i=mParentJoint->getNumTransforms()-1; i>=0; i--){
		if(mParentJoint->getTransform(i)->isPresent(d)){
			mParentJoint->getTransform(i)->applyDerivativeMat(d, &result);
			d->setDerivFlag(true);
		}
		else {
			mParentJoint->getTransform(i)->applyTransformMat(&result);
		}
	}
	if(mParentJoint->getBodyIn()==NULL) {
		if (d->getDerivFlag()) {
			d->setDerivFlag(false);	// restore to false
			return result;
		}
		else {
			Vec4d zero = vl_0;
			return zero;
		}
	}
	else return mParentJoint->getBodyIn()->getFirstDerivative(&result, d);
}

Vec4d BodyLink::approxCOMFirstDerivative(Vec4d* h, Dof* d)
{
	Vec4d localPos = *h;
	Vec4d worldPos = getWorldCoords(&localPos);

	double value = d->getValue();
	const double diff = 0.001;
	d->setValue(value + diff);

	Vec4d currentWorldPos = getWorldCoords(&localPos);
	d->setValue(value);
	return (currentWorldPos - worldPos) / diff;
}

Mat4d BodyLink::approxRotFirstDerivative(Mat4d* h, Dof* d)
{
	Mat4d worldTrans = getWorldTransform();

	double value = d->getValue();
	const double diff = 0.0001;
	d->setValue(value + diff);

	Mat4d currentWorldTrans = getWorldTransform();
	d->setValue(value);
	return (currentWorldTrans - worldTrans) / diff;
}

Mat4d BodyLink::getFirstDerivative(Mat4d *h, Dof *d){
	Mat4d result = *h;
	for(int i=mParentJoint->getNumTransforms()-1; i>=0; i--){
		if(mParentJoint->getTransform(i)->isPresent(d)){
			mParentJoint->getTransform(i)->applyDerivativeMat(d, &result);
			d->setDerivFlag(true);
		}
		else {
			mParentJoint->getTransform(i)->applyTransformMat(&result);
		}
	}
	if(mParentJoint->getBodyIn()==NULL) {
		if (d->getDerivFlag()) {
			d->setDerivFlag(false);	// restore to false
			return result;
		}
		else {
			Mat4d zero = vl_0;
			return zero;
		}
	}
	else return mParentJoint->getBodyIn()->getFirstDerivative(&result, d);
}

Mat4d BodyLink::getLocalFirstDerivative(Mat4d *h, Dof *d){
	Mat4d result = *h;
	for(int i=mParentJoint->getNumTransforms()-1; i>=0; i--){
		if(mParentJoint->getTransform(i)->isPresent(d)){
			mParentJoint->getTransform(i)->applyDerivativeMat(d, &result);
			d->setDerivFlag(true);
		}
		else {
			mParentJoint->getTransform(i)->applyTransformMat(&result);
		}
	}
	if(d->getDerivFlag()){
		d->setDerivFlag(false);	// restore to false
		return result;
	}
	else {
		printf("BodyLink::getLocalFirstDerivative - Should not have been called as no deriv exists\n");
		Mat4d zero = vl_0;
		return zero;
	}
}

Vec4d BodyLink::getSecondDerivative(Vec4d *h, Dof *d1, Dof *d2){
	Vec4d result = *h;
	for(int i=mParentJoint->getNumTransforms()-1; i>=0; i--){
		if(mParentJoint->getTransform(i)->isPresent(d1)){
			// both d1 d2 present in transformation
			if(mParentJoint->getTransform(i)->isPresent(d2)){
				mParentJoint->getTransform(i)->applyDerivativeMat(d1, d2, &result);
				d1->setDerivFlag(true);
				d2->setDerivFlag(true);
			}
			// only d1 present in transformation
			else {
				mParentJoint->getTransform(i)->applyDerivativeMat(d1, &result);
				d1->setDerivFlag(true);
			}
		}
		// only d2 present in transformation
		else if(mParentJoint->getTransform(i)->isPresent(d2)){
			mParentJoint->getTransform(i)->applyDerivativeMat(d2, &result);
			d2->setDerivFlag(true);
		}
		// none present in transformation
		else {
			mParentJoint->getTransform(i)->applyTransformMat(&result);
		}
	}
	if(mParentJoint->getBodyIn()==NULL) {
		if (d1->getDerivFlag() && d2->getDerivFlag()) {
			d1->setDerivFlag(false);	// restore to false
			d2->setDerivFlag(false);	// restore to false
			return result;
		}
		else {
			d1->setDerivFlag(false);	// restore to false
			d2->setDerivFlag(false);	// restore to false
			Vec4d zero = vl_0;
			return zero;
		}
	}
	else return mParentJoint->getBodyIn()->getSecondDerivative(&result, d1, d2);
}

Mat4d BodyLink::getSecondDerivative(Mat4d *h, Dof *d1, Dof *d2){
	Mat4d result = *h;
	for(int i=mParentJoint->getNumTransforms()-1; i>=0; i--){
		if(mParentJoint->getTransform(i)->isPresent(d1)){
			// both d1 d2 present in transformation
			d1->setDerivFlag(true);
			if(mParentJoint->getTransform(i)->isPresent(d2)){
				mParentJoint->getTransform(i)->applyDerivativeMat(d1, d2, &result);
				d2->setDerivFlag(true);
			}
			// only d1 present in transformation
			else {
				mParentJoint->getTransform(i)->applyDerivativeMat(d1, &result);
			}
		}
		// only d2 present in transformation
		else if(mParentJoint->getTransform(i)->isPresent(d2)){
			mParentJoint->getTransform(i)->applyDerivativeMat(d2, &result);
			d2->setDerivFlag(true);
		}
		// none present in transformation
		else {
			mParentJoint->getTransform(i)->applyTransformMat(&result);
		}
	}
	if(mParentJoint->getBodyIn()==NULL) {
		if (d1->getDerivFlag() && d2->getDerivFlag()) {
			d1->setDerivFlag(false);	// restore to false
			d2->setDerivFlag(false);	// restore to false
			return result;
		}
		else {
			d1->setDerivFlag(false);	// restore to false
			d2->setDerivFlag(false);	// restore to false
			Mat4d zero = vl_0;
			return zero;
		}
	}
	else return mParentJoint->getBodyIn()->getSecondDerivative(&result, d1, d2);
}

void BodyLink::draw(Vec4d _color, bool _default ){
	glPushMatrix();
	// render the self geometry
	for(int i=0; i<mParentJoint->getNumTransforms(); i++){
		mParentJoint->getTransform(i)->applyGLTransform();
	}
	if(mPrimitive != NULL) {
		glPushName((unsigned)mID);
		mPrimitive->draw(_color, _default);
		glPopName();
	}

	// render the subtree
	for(int i=0; i<mChildJoints.size(); i++){
		mChildJoints[i]->getBodyOut()->draw(_color, _default);
	}
	glPopMatrix();
}

void BodyLink::drawHandles(Vec4d _color, bool _default){
	glPushMatrix();
	for(int i=0; i<mParentJoint->getNumTransforms(); i++){
		mParentJoint->getTransform(i)->applyGLTransform();
	}

	// render the corresponding mHandless
	for(int i=0; i<mHandles.size(); i++){
		mHandles[i]->draw(false, _color, _default);
	}
	for(int i=0; i<mChildJoints.size(); i++){
		mChildJoints[i]->getBodyOut()->drawHandles(_color, _default);
	}
	glPopMatrix();
}

void BodyLink::setGLTransform (const double pos[3], const double R[12])
{
	GLfloat matrix[16];
	matrix[0]=R[0];
	matrix[1]=R[4];
	matrix[2]=R[8];
	matrix[3]=0;
	matrix[4]=R[1];
	matrix[5]=R[5];
	matrix[6]=R[9];
	matrix[7]=0;
	matrix[8]=R[2];
	matrix[9]=R[6];
	matrix[10]=R[10];
	matrix[11]=0;
	matrix[12]=pos[0];
	matrix[13]=pos[1];
	matrix[14]=pos[2];
	matrix[15]=1;
	//glPushMatrix();
	glMultMatrixf(matrix);
}

void BodyLink::setSubtreeMass()
{
	double mass = mPrimitive->getMass();
	for(int i=0; i<mChildJoints.size(); i++){
		BodyLink* child = mChildJoints[i]->getBodyOut();
		child->setSubtreeMass();
		mass += child->getSubtreeMass();
	}
	mSubtreeMass = mass;
}

Vec3d BodyLink::computeSubtreeCOM()
{
	Vec4d com(mPrimitive->getOffset(), 1);
	com = getWorldTransform()*com;
	Vec3d result(com[0], com[1], com[2]);
	result = result * mPrimitive->getMass();
	for(int i=0; i<mChildJoints.size(); i++){
		BodyLink *child = mChildJoints[i]->getBodyOut();
		result += child->computeSubtreeCOM() * child->getSubtreeMass();
	}
	return result/mSubtreeMass;
}
